/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2022 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "bsp_dwt.h"
#include "motor_simulation.h"
#include "PID.h"
#include "string_generate.h"
#include <stdio.h>
#include <math.h>
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define pi 3.1415
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
uint32_t DWT_CNT;
uint32_t CNT_printf;
uint32_t CNT_while;
uint32_t CNT_pid;
uint32_t CNT_motor;

uint8_t Sendbuf[500];

float dt;
float Input_1 = 0 , Input_2 = 0;
float printf_dt = 0 , while_dt = 0 , pid_dt = 0 , motor_dt = 0;

motorObject_t Motor1;
motorObject_t Motor2;
motorObject_t Motor3;

PID_t PID_Velocity_1;
PID_t PID_Angle_1;
PID_t PID_Velocity_2;
PID_t PID_Angle_2;


float w1 = 2*pi*6.892 , w2 = 2*pi*1.0153 , w3 = 2*pi*1.0232 , w4 = 2*pi*0.9635 , T;
float VelocityRef = 10;
float AngleRef = 2 * pi ;
float disturbance = 10;
float Current_1, Velocity_1, Angle_1;
float Current_2, Velocity_2, Angle_2;

uint8_t receive_data[500];
int len;
float pid_param_buf[6];
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
void Rx_PID_Init(uint8_t motor_num , float rx_pid_param[]);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  /* USER CODE BEGIN 2 */

  //使能串口空闲中断
	__HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
	HAL_UART_Receive_DMA(&huart1, receive_data, 100);
  
	//DWT初始化
  DWT_Init(72);
	
  //电机模拟初始化
  Motor_Object_Init(&Motor1);
	Motor_Object_Init(&Motor2);
	
  //PID初始化
	//单级参数
		//mode = pid_position
	//pid_init(&PID_Angle_1 , 22.45 , 0.070 , 1.746 , 0);
  //pid_init(&PID_Velocity_1 , 10 , 25.8 , 0 , 0);

		//mode = pid_delta  不太行
	//pid_init(&PID_Angle , 30	, 0.0076 , 0.01 , 1);
  //pid_init(&PID_Velocity , 10 , 25.8 , 0 , 1);

	//串级参数
  pid_init(&PID_Angle_1 , 24.125 , 0.005 , 0 , 0);
  pid_init(&PID_Velocity_1 , 8 , 0.005 , 0 , 0);
	
  pid_init(&PID_Angle_2 , 24.125 , 0.005 , 0 , 0);
  pid_init(&PID_Velocity_2 , 8 , 0.005 , 0 , 0);
	
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
while (1)
{	
	while_dt = DWT_GetDeltaT(&CNT_while);
  dt =  DWT_GetDeltaT(&DWT_CNT);
		
  Current_1 = Get_Motor_Current(&Motor1);
  Velocity_1 = Get_Motor_Velocity(&Motor1);
  Angle_1 = Get_Motor_Angle(&Motor1);

  Current_2 = Get_Motor_Current(&Motor2);
  Velocity_2 = Get_Motor_Velocity(&Motor2);
  Angle_2 = Get_Motor_Angle(&Motor2);
	
  //频率响应
  //T += dt;
  //VelocityRef = 10*(sin(w1*T));
  //AngleRef = 2*pi*(sin(w2*T));
  //AngleRef = 2*pi*(sin(w3*T));
	//AngleRef = 2*pi*(sin(w4*T));
		 
  //单级pid
  //Input = pid_calculate(&PID_Velocity , Velocity , VelocityRef , dt);
  //Input = pid_calculate(&PID_Angle , Angle , AngleRef , dt);
	
  //串级pid
  Input_2 = pid_cascade_calculate(&PID_Angle_2 , &PID_Velocity_2 , Angle_2 , Velocity_2 , AngleRef , dt);
  Input_1 = pid_cascade_calculate_feedforward(&PID_Angle_1 , &PID_Velocity_1 , Angle_1 , Velocity_1 , AngleRef , dt , 1);
	
	
  //按键干扰
  if(HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_13) == 0)
  {
    Input_1 = disturbance;
    Input_2 = disturbance;
  }
	
  //电机模拟
	DWT_GetDeltaT(&CNT_motor);
  Motor_Simulation(&Motor1, Input_1, dt);
  Motor_Simulation(&Motor2, Input_2, dt);
	motor_dt = DWT_GetDeltaT(&CNT_motor);
		
  //DMA串口发送
  DWT_GetDeltaT(&CNT_printf);
  uint8_t parameter_title[][10] ={"angle" , "angle1"};
	String_Generate(&Sendbuf[0] , 2 , parameter_title , Angle_1 , Angle_2);
	printf_dt = DWT_GetDeltaT(&CNT_printf);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
	HAL_Delay(1);
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
void UART_IDLE_Callback(UART_HandleTypeDef *huart)
{
  if(__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE))     //判断一帧数据是否接收完毕
  {
		__HAL_UART_CLEAR_IDLEFLAG(huart);     //清空IDLE标志位
		(void)USART1->SR;      //清空SR寄存器
		(void)USART1->DR;     //清空DR寄存器
		__HAL_DMA_CLEAR_FLAG(huart, DMA_FLAG_TC5);    //清空DMA传输完成标志位
		HAL_UART_DMAStop(huart);
    uint8_t motor_num;
		// 接收到的字节数
		len = 100 - __HAL_DMA_GET_COUNTER(huart->hdmarx);
    motor_num = receive_data[0];
    Rx_PID_Param(huart , receive_data , len , pid_param_buf);
    Rx_PID_Init(motor_num , pid_param_buf);
		HAL_UART_Receive_DMA(huart,receive_data,100);		//再次使能接收，NDTR重载
	}
}

void Rx_PID_Init(uint8_t motor_num , float rx_pid_param[])
{
  //可根据后续电机数以及编号而添加
  switch (motor_num)
  {
  case '1': pid_init(&PID_Angle_1 , rx_pid_param[0] , rx_pid_param[1] , rx_pid_param[2] , 0);
            pid_init(&PID_Velocity_1 , rx_pid_param[3] , rx_pid_param[4] , rx_pid_param[5] , 0);
    break;
  case '2': pid_init(&PID_Angle_2 , rx_pid_param[0] , rx_pid_param[1] , rx_pid_param[2] , 0);
            pid_init(&PID_Velocity_2 , rx_pid_param[3] , rx_pid_param[4] , rx_pid_param[5] , 0);
    break;
  }
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */

  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
